using System;
using System.IO;
using Game.Lib.CSG;

namespace Game.Lib
{
    public class Quaternion : Vector
    {
        #region Constants
        
        /// <summary>
        /// The quaternion representing zero rotation
        /// </summary>
        public static Quaternion Zero = new Quaternion();
        
        #endregion
        
        #region Fields
        
        public float X;
        public float Y;
        public float Z;
        public float W;
        
        #endregion
        
        #region Constructors
        
        public Quaternion()
        {
            X = Y = Z = 0f;
            W = 1f;
        }
        
        public Quaternion(float x, float y, float z, float w)
        {
            X = x;
            Y = y;
            Z = z;
            W = w;
        }
        
        public Quaternion(float roll, float pitch, float yaw)
        {
            double atCos = Math.Cos(roll / 2f);
            double atSin = Math.Sin(roll / 2f);
            double leftCos = Math.Cos(pitch / 2f);
            double leftSin = Math.Sin(pitch / 2f);
            double upCos = Math.Cos(yaw / 2f);
            double upSin = Math.Sin(yaw / 2f);
            double atLeftCos = atCos * leftCos;
            double atLeftSin = atSin * leftSin;
            X = (float)(atSin * leftCos * upCos + atCos * leftSin * upSin);
            Y = (float)(atCos * leftSin * upCos - atSin * leftCos * upSin);
            Z = (float)(atLeftCos * upSin + atLeftSin * upCos);
            W = (float)(atLeftCos * upCos - atLeftSin * upSin);
        }

        public Quaternion(Vector3 axis, float angle)
        {
            axis.Normalize();

            angle *= .5f;
            float s = (float)Math.Sin(angle);

            X = axis.X * s;
            Y = axis.Y * s;
            Z = axis.Z * s;
            W = (float)Math.Cos(angle);

            Normalize();
        }
        
        public Quaternion(Vector3 angles)
        {
            double atCos = Math.Cos(angles.X * .5f);
            double atSin = Math.Sin(angles.X * .5f);
            double leftCos = Math.Cos(angles.Y * .5f);
            double leftSin = Math.Sin(angles.Y * .5f);
            double upCos = Math.Cos(angles.Z * .5f);
            double upSin = Math.Sin(angles.Z * .5f);
            double atLeftCos = atCos * leftCos;
            double atLeftSin = atSin * leftSin;
            X = (float)(atSin * leftCos * upCos + atCos * leftSin * upSin);
            Y = (float)(atCos * leftSin * upCos - atSin * leftCos * upSin);
            Z = (float)(atLeftCos * upSin + atLeftSin * upCos);
            W = (float)(atLeftCos * upCos - atLeftSin * upSin);
        }
        
        #endregion
        
        #region Methods
        
        public void Normalize()
        {
            float q = Magnitude();
            if(q > .000001f)
            {
	            float sc = 1f / q;
	            X *= sc;
	            Y *= sc;
	            Z *= sc;
	            W *= sc;
            }
            else
            {
                X = Y = Z = 0f;
                W = 1f;
            }
        }

        public float Magnitude()
        {
            return (float)Math.Sqrt(X * X + Y * Y + Z * Z + W * W);
        }
        
        public float MagnitudeSquared()
        {
            return X * X + Y * Y + Z * Z + W * W;
        }
        
        public Vector3 ToEuler()
        {
            float pitch = 0f;
            float roll = 0f;
            float yaw = 0f;
            
            Quaternion t = new Quaternion(this.X * this.X, this.Y * this.Y, this.Z * this.Z, this.W * this.W);

            float m = t.X + t.Y + t.Z + t.W;
            if (Math.Abs(m) < 0.001d) return new Vector3(0f, 0f, 0f);
            float n = 2f * (this.Y * this.W + this.X * this.Z);
            float p = m * m - n * n;

            if (p > 0f)
            {
                roll = (float)Math.Atan2(2.0f * (this.X * this.W - this.Y * this.Z), (-t.X - t.Y + t.Z + t.W));
                pitch = (float)Math.Atan2(n, Math.Sqrt(p));
                yaw = (float)Math.Atan2(2.0f * (this.Z * this.W - this.X * this.Y), t.X - t.Y - t.Z + t.W);
            }
            else if (n > 0f)
            {
                roll = 0f;
                pitch = (float)(Math.PI / 2d);
                yaw = (float)Math.Atan2((this.Z * this.W + this.X * this.Y), 0.5f - t.X - t.Y);
            }
            else
            {
                roll = 0f;
                pitch = -(float)(Math.PI / 2d);
                yaw = (float)Math.Atan2((this.Z * this.W + this.X * this.Y), 0.5f - t.X - t.Z);
            }
            
            return new Vector3(roll, pitch, yaw);
        }
        
        public void ToAxisAngle(out Vector3 axis, out float angle)
        {
            axis = new Vector3();
            float scale = (float)Math.Sqrt(X * X + Y * Y + Z * Z);

            if (scale < Single.Epsilon || W > 1.0f || W < -1.0f)
            {
                angle = 0.0f;
                axis.X = 0.0f;
                axis.Y = 1.0f;
                axis.Z = 0.0f;
            }
            else
            {
                angle = 2.0f * (float)Math.Acos(W);
                float ooscale = 1f / scale;
                axis.X = X * ooscale;
                axis.Y = Y * ooscale;
                axis.Z = Z * ooscale;
            }
        }

        public override int GetHashCode()
        {
            return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode() ^ W.GetHashCode();
        }

        public override string ToString()
        {
            return String.Format(Utils.EnUsCulture, "<{0}, {1}, {2}, {3}>", X, Y, Z, W);
        }

        public override bool Equals(object obj)
        {
            if(!(obj is Quaternion)) return false;
            return this == (Quaternion)obj;
        }
        
        #endregion
        
        #region Statics
        
        public static Quaternion Parse(string p)
        {
            string[] split = p.Replace("<", String.Empty).Replace(">", String.Empty).Split(new char[] { ',' });
            if(split.Length == 3)
            {
                return new Quaternion(
                    float.Parse(split[0].Trim(), Utils.EnUsCulture),
                    float.Parse(split[1].Trim(), Utils.EnUsCulture),
                    float.Parse(split[2].Trim(), Utils.EnUsCulture));
            }
            else if(split.Length == 4)
            {
                return new Quaternion(
                    float.Parse(split[0].Trim(), Utils.EnUsCulture),
                    float.Parse(split[1].Trim(), Utils.EnUsCulture),
                    float.Parse(split[2].Trim(), Utils.EnUsCulture),
                    float.Parse(split[3].Trim(), Utils.EnUsCulture));
            }
            else throw new ArgumentException("Quaternions must be of the form \"<X, Y, Z, W>\"");
        }
        
        #endregion
        
        #region Operators
        
        public static Quaternion operator +(Quaternion a, Quaternion b)
        {
            a.X += b.X;
            a.Y += b.Y;
            a.Z += b.Z;
            a.W += b.W;
            return a;
        }
        
        public static Quaternion operator -(Quaternion a, Quaternion b)
        {
            a.X -= b.X;
            a.Y -= b.Y;
            a.Z -= b.Z;
            a.W -= b.W;
            return a;
        }
        
        public static Quaternion operator *(Quaternion a, Quaternion b)
        {
            return new Quaternion(
                a.W * b.X + a.X * b.W + a.Y * b.Z - a.Z * b.Y,
                a.W * b.Y + a.Y * b.W + a.Z * b.X - a.X * b.Z,
                a.W * b.Z + a.Z * b.W + a.X * b.Y - a.Y * b.X,
                a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z
            );
        }
        
        public static Quaternion operator /(Quaternion a, Quaternion b)
        {
            float x = a.X;
            float y = a.Y;
            float z = a.Z;
            float w = a.W;
            
            b = ~b * (1f / b.MagnitudeSquared());

            float x2 = b.X;
            float y2 = b.Y;
            float z2 = b.Z;
            float w2 = b.W;

            return new Quaternion(
                ((x * w2) + (x2 * w)) + (y * z2) - (z * y2),
                ((y * w2) + (y2 * w)) + (z * x2) - (x * z2),
                ((z * w2) + (z2 * w)) + (x * y2) - (y * x2),
                (w * w2) - ((x * x2) + (y * y2)) + (z * z2));
            
        }
        
        public static Quaternion operator *(Quaternion a, float s)
        {
            a.X *= s;
            a.Y *= s;
            a.Z *= s;
            a.W *= s;
            return a;
        }
        
        public static Quaternion operator *(float s, Quaternion a)
        {
            a.X *= s;
            a.Y *= s;
            a.Z *= s;
            a.W *= s;
            return a;
        }
        
        public static bool operator ==(Quaternion a, Quaternion b)
        {
            return  a.X == b.X &&
                    a.Y == b.Y &&
                    a.Z == b.Z &&
                    a.W == b.W;
        }
        
        public static bool operator !=(Quaternion a, Quaternion b)
        {
            return  a.X != b.X ||
                    a.Y != b.Y ||
                    a.Z != b.Z ||
                    a.W != b.W;
        }
        
        public static Quaternion operator ~(Quaternion a)
        {
            a.X = -a.X;
            a.Y = -a.Y;
            a.Z = -a.Z;
            return a;
        }
        
        public static Quaternion operator -(Quaternion a)
        {
            a.X = -a.X;
            a.Y = -a.Y;
            a.Z = -a.Z;
            a.W = -a.W;
            return a;
        }
        
        #endregion

        public void FromBytes(byte[] data, int pos)
        {
            if(!BitConverter.IsLittleEndian)
            {
                Array.Reverse(data, pos, 4);
                Array.Reverse(data, pos + 4, 4);
                Array.Reverse(data, pos + 8, 4);
                Array.Reverse(data, pos + 12, 4);
            }
            X = BitConverter.ToSingle(data, pos);
            Y = BitConverter.ToSingle(data, pos + 4);
            Z = BitConverter.ToSingle(data, pos + 8);
            W = BitConverter.ToSingle(data, pos + 12);
        }

        public byte[] ToBytes()
        {
            byte[] buf = new byte[16];
            Buffer.BlockCopy(BitConverter.GetBytes(X), 0, buf, 0, 4);
            Buffer.BlockCopy(BitConverter.GetBytes(Y), 0, buf, 4, 4);
            Buffer.BlockCopy(BitConverter.GetBytes(Z), 0, buf, 8, 4);
            Buffer.BlockCopy(BitConverter.GetBytes(W), 0, buf, 12, 4);
            if(!BitConverter.IsLittleEndian)
            {
                Array.Reverse(buf, 0, 4);
                Array.Reverse(buf, 4, 4);
                Array.Reverse(buf, 8, 4);
                Array.Reverse(buf, 12, 4);
            }
            return buf;
        }

        public void Serialize(Stream s)
        {
            if(!s.CanWrite) throw new ArgumentException("Serialization requires a writable stream.");
            s.Write(ToBytes(), 0, 16);
        }

        public void Deserialize(Stream s)
        {
            byte[] buf = new byte[16];
            int len = s.Read(buf, 0, 16);
            if(len != 16) throw new EndOfStreamException("Insuffecient data to reconstuite quaternion");
            if(!BitConverter.IsLittleEndian)
            {
                Array.Reverse(buf, 0, 4);
                Array.Reverse(buf, 4, 4);
                Array.Reverse(buf, 8, 4);
                Array.Reverse(buf, 12, 4);
            }
            X = BitConverter.ToSingle(buf, 0);
            Y = BitConverter.ToSingle(buf, 4);
            Z = BitConverter.ToSingle(buf, 8);
            W = BitConverter.ToSingle(buf, 12);
        }
    }
}
